#ifndef ARMCONTROLLERIMPL_H
#define ARMCONTROLLERIMPL_H
//#include "base_impl.h"
#include <memory>
#include "Eigen/Eigen"
#include <vector>
#include <iostream>
#include "communication/typetransfer.h"
#include "communication/utility.hpp"
#include <predef.h>
class ArmControllerImpl// : public BaseImpl
{
public:
    ArmControllerImpl(const u_char index,const int joint_num=7);
    virtual ~ArmControllerImpl();
    bool ParseUserData(const u_char *buffer, const size_t buffer_len);
    bool PackUserData(u_char *buffer, size_t &len);
    //size_t PackUserData(u_char *buffer);

//    inline SlaveType GetType()const {return kArm;}
    inline int GetJointNum()const{return joint_num_;}

//    void ResetArmTempOriginPos()
//    {
//        arm_end_temp_origin_pos_=arm_end_origin_pos_;
//    }
//    void SetArmOriginPos(const Eigen::Vector3d &pos)
//    {
//        arm_end_origin_pos_=pos;
//        arm_end_temp_origin_pos_=pos;
//    }
//    void SetArmTempOriginPos(const Eigen::Vector3d &pos)
//    {
//        arm_end_temp_origin_pos_=pos;
//    }
    //inline const Eigen::Vector3d &GetArmOriginPos()const{return arm_end_origin_pos_;}

    inline const Eigen::Vector3d &GetArmCurEndPos()const{return arm_end_pos_cur_;}
    inline const Eigen::Quaterniond &GetArmCurEndQuat()const{return arm_end_quat_cur_;}
    inline const Eigen::Vector3d &GetArmCurEndLinearVel()const{return arm_end_v_cur_;}
    inline const Eigen::Vector3d &GetArmCurEndAngularVel()const{return arm_end_w_cur_;}
    inline const Eigen::VectorXd &GetArmCurEndWrench()const{return arm_end_wrench_cur_;}

    inline double GetFingerCurJointPos()const{return finger_joint_pos_cur_;}
    inline double GetFingerCurJointVel()const{return finger_joint_vel_cur_;}
    inline double GetFingerCurJointForce()const{return finger_joint_force_cur_;}


    inline const Eigen::Vector3d &GetArmTargetEndPos()const{return arm_end_pos_target_;}
    inline const Eigen::Quaterniond &GetArmTargetEndQuat()const{return arm_end_quat_target_;}
    inline const Eigen::Vector3d &GetArmTargetEndLinearVel()const{return arm_end_v_target_;}
    inline const Eigen::Vector3d &GetArmTargetEndAngularVel()const{return arm_end_w_target_;}
    inline double GetFingerTargetJointPos()const{return finger_joint_pos_target_;}
    inline double GetFingerTargetJointVel()const{return finger_joint_vel_target_;}

    inline void ClearTargetStatus(){ updated_attr_=0x00;}
    /*
    bool PackSocketMsg(char *data,size_t &byte_num,const int msg_id)const
    {
        if(target_updated_==0x00)
        {
            byte_num=0;
            return false;
        }
        data[0]=0x42;
        data[1]=0x53;
        data[2]=0x03;
        data[3]=0x00;
        data[4]=0x00;
        TypeTransfer<int>::Type2ByteArray(data+5,msg_id,2);
        byte_num=7;

        TypeTransfer<u_char>::Type2ByteArray(data+byte_num,index_);
        byte_num+=sizeof (u_char);
        //0x01 pos  0x02 ori  0x04 vel    0x08 angular vel
        TypeTransfer<char>::Type2ByteArray(data+byte_num,target_updated_);
        byte_num+=sizeof (char);
        if(target_updated_&kEndPos)
        {
            TypeTransfer<double>::Type2ByteArray(data+byte_num,arm_end_pos_target_[0]);
            byte_num+=sizeof (double);
            TypeTransfer<double>::Type2ByteArray(data+byte_num,arm_end_pos_target_[1]);
            byte_num+=sizeof (double);
            TypeTransfer<double>::Type2ByteArray(data+byte_num,arm_end_pos_target_[2]);
            byte_num+=sizeof (double);
        }
        if(target_updated_&kEndQuat)
        {
            TypeTransfer<double>::Type2ByteArray(data+byte_num,arm_end_quat_target_.w());
            byte_num+=sizeof (double);
            TypeTransfer<double>::Type2ByteArray(data+byte_num,arm_end_quat_target_.x());
            byte_num+=sizeof (double);
            TypeTransfer<double>::Type2ByteArray(data+byte_num,arm_end_quat_target_.y());
            byte_num+=sizeof (double);
            TypeTransfer<double>::Type2ByteArray(data+byte_num,arm_end_quat_target_.z());
            byte_num+=sizeof (double);
        }
        if(target_updated_&kEndLinearVel)
        {
            TypeTransfer<double>::Type2ByteArray(data+byte_num,arm_end_v_target_[0]);
            byte_num+=sizeof (double);
            TypeTransfer<double>::Type2ByteArray(data+byte_num,arm_end_v_target_[1]);
            byte_num+=sizeof (double);
            TypeTransfer<double>::Type2ByteArray(data+byte_num,arm_end_v_target_[2]);
            byte_num+=sizeof (double);
        }
        if(target_updated_&kEndAngularVel)
        {
            TypeTransfer<double>::Type2ByteArray(data+byte_num,arm_end_w_target_[0]);
            byte_num+=sizeof (double);
            TypeTransfer<double>::Type2ByteArray(data+byte_num,arm_end_w_target_[1]);
            byte_num+=sizeof (double);
            TypeTransfer<double>::Type2ByteArray(data+byte_num,arm_end_w_target_[2]);
            byte_num+=sizeof (double);
        }
        //target_updated_=0x00;

        int len=byte_num-5+2;
        TypeTransfer<int>::Type2ByteArray(data+3,len,2);

        //cout<<"available data len:"<<len<<endl;
        u_int16_t crc=CalCRC16(data,byte_num);
        //cout<<"send crc:"<<crc<<endl;
        TypeTransfer<u_int16_t>::Type2ByteArray(data+byte_num,crc);
        byte_num+=2;
        return true;
    }
    */


    /*
     * target setting from master
     */
    //descartes space
    //enum ArmAttrType{kEndPos=0x01,kEndQuat=0x02,kEndLinearVel=0x04,kEndAngularVel=0x08,kGripperPos=0x10,kGripperVel=0x20,kJointPos=0x40,kJointVel=0x80};
    bool SetArmTargetEndPos(const Eigen::Vector3d &p)
    {
        //target_end_pos_ = mapping_ori_m2s_*Eigen::Vector3d(px,py,pz)*mapping_coeff_m2s_+slave_rela_origin_pos_;
        arm_end_pos_target_=p;
        updated_attr_=updated_attr_|Master2Slave::kEndPos;
        return true;
    }
    bool SetArmTargetEndPos(const double &px,const double &py,const double &pz)
    {
        //target_end_pos_ = mapping_ori_m2s_*Eigen::Vector3d(px,py,pz)*mapping_coeff_m2s_+slave_rela_origin_pos_;
        arm_end_pos_target_<<px,py,pz;
        updated_attr_=updated_attr_|Master2Slave::kEndPos;
        return true;
    }
    bool SetArmTargetEndQuat(const Eigen::Quaterniond &q)
    {
        //target_end_pos_ = mapping_ori_m2s_*Eigen::Vector3d(px,py,pz)*mapping_coeff_m2s_+slave_rela_origin_pos_;
        arm_end_quat_target_=q;
        updated_attr_=updated_attr_|Master2Slave::kEndQuat;
        return true;
    }
    bool SetArmTargetEndQuat(const double &w,const double &ax,const double &ay,const double &az)
    {
        //target_end_quat_= mapping_ori_m2s_*Eigen::Quaterniond(w,ax,ay,az);
        arm_end_quat_target_.coeffs()<<ax,ay,az,w;
        updated_attr_=updated_attr_|Master2Slave::kEndQuat;
        return true;
    }
    bool SetArmTargetEndPose(const double &px,const double &py,const double &pz,const double &w,const double &ax,const double &ay,const double &az)
    {
        SetArmTargetEndPos(px,py,pz);
        SetArmTargetEndQuat(w,ax,ay,az);
        return true;
    }
    bool SetArmTargetEndPose(const Eigen::Matrix4d& pose)
    {
        SetArmTargetEndPos(pose(0,3),pose(1,3),pose(2,3));
        Eigen::Quaterniond q(static_cast<Eigen::Matrix3d>(pose.block(0,0,3,3)));//Eigen::Quaterniond::Identity();
        SetArmTargetEndQuat(q.w(),q.x(),q.y(),q.z());
        //std::cout<<"slave_target"<<std::endl<<pose<<std::endl<<std::endl;
        return true;
    }
    bool SetArmTargetEndVel(const Eigen::Vector3d &v,const Eigen::Vector3d &w)
    {
        //target_end_v_= mapping_ori_m2s_*Eigen::Vector3d(vx,vy,vz);
        arm_end_v_target_=v;
        arm_end_w_target_=w;
        updated_attr_=updated_attr_|Master2Slave::kEndLinearVel|Master2Slave::kEndAngularVel;
        return true;
    }
    bool SetArmTargetEndVel(const double &vx,const double &vy,const double &vz, const double &wx,const double &wy,const double &wz)
    {
        //target_end_v_= mapping_ori_m2s_*Eigen::Vector3d(vx,vy,vz);
        arm_end_v_target_<<vx,vy,vz;
        arm_end_w_target_<<wx,wy,wz;
        updated_attr_=updated_attr_|Master2Slave::kEndLinearVel|Master2Slave::kEndAngularVel;
        return true;
    }

    inline void SetFingerTargetJointPos(const double &joints_pos)
    {
        finger_joint_pos_target_ = joints_pos;
        updated_attr_=updated_attr_|Master2Slave::kGripperPos;
    }
    inline void SetFingerTargetJointVel(const double &joints_vel)
    {
        finger_joint_vel_target_ = joints_vel;
        updated_attr_=updated_attr_|Master2Slave::kGripperVel;
    }
    inline void SetArmTargetJointPos(std::vector<double> &joints_pos)
    {
        arm_joint_pos_target_ = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(joints_pos.data(),joints_pos.size());
        updated_attr_=updated_attr_|Master2Slave::kJointPos;
    }
    inline void SetArmTargetJointVel(std::vector<double> &joints_vel)
    {
        arm_joint_vel_target_ = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(joints_vel.data(),joints_vel.size());
        updated_attr_=updated_attr_|Master2Slave::kJointVel;
    }
    //target from master end


private://from slave via socket
    const int joint_num_;
    // enum ArmAttrType{kEndPos=0x01,kEndQuat=0x02,kEndLinearVel=0x04,kEndAngularVel=0x08,kEndForce=0x10,kJointPos=0x20,kJointVel=0x40,kJointForce=0x80};
    Eigen::Vector3d arm_end_pos_cur_;
    Eigen::Quaterniond arm_end_quat_cur_;
    Eigen::Vector3d arm_end_v_cur_;
    Eigen::Vector3d arm_end_w_cur_;
    Eigen::VectorXd arm_end_wrench_cur_;

    Eigen::VectorXd arm_joint_pos_cur_;
    Eigen::VectorXd arm_joint_vel_cur_;
    Eigen::VectorXd arm_joint_force_cur_;

    //enum HandAttrType{kGripperPos=0x01,kGripperVel=0x02,kGripperForce=0x04};
    double finger_joint_pos_cur_;
    double finger_joint_vel_cur_;
    double finger_joint_force_cur_;


public:
    u_char updated_attr_;
private://from master
    Eigen::Vector3d arm_end_pos_target_;
    Eigen::Quaterniond arm_end_quat_target_;
    Eigen::Vector3d arm_end_v_target_;
    Eigen::Vector3d arm_end_w_target_;

    double finger_joint_pos_target_;
    double finger_joint_vel_target_;
private://from master (joint space control)
    Eigen::VectorXd arm_joint_pos_target_;
    Eigen::VectorXd arm_joint_vel_target_;

    u_char target_index_;
    //dh param
    //fk
};
typedef std::shared_ptr<ArmControllerImpl> ArmControllerImplPtr;

#endif // ARMCONTROLLERIMPL_H
